/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include "linelib.h"
#include "mapHandling.h"

//#define MAIN

/**
 * Maximum mapsize
 */
#ifndef MAX_MAPSIZE
	#define MAX_MAPSIZE	4000
#endif

/**
* TLineVector readMapDataFromFile(const char *fileName,float scale)
* 
* Reads a lise map from file. Map is in format Nx4
* MAX_MAPSIZE defines the maximum number of used lines
* scale is the conversion scale between the units in file and meter
* e.g. usually cad drawings are in mm:s so the conversion factor is 1/1000
* 
* returns line vector, which contains all lines (and its parametric represantation)
*/
TLineVector readMapDataFromFile(const char *fileName,float scale){

	//FILE *mapfile	  = fopen("mapAutLab.txt",    "r");	   // Map data as lines
	FILE *mapfile	  = fopen(fileName,    "r");	   // Map data as line
	
	int k=0, i=0;
	float map[4 * MAX_MAPSIZE];
	float maplines[3 * MAX_MAPSIZE];
	TLineVector cMap;

	// Check parameters
	if(mapfile==NULL){
		cMap.NumOfLines=-1;
//		error(ERRS, "readMapDataFromFile: %s file not found.");
		return cMap;
	}
	// Read map from file
	k = 0;
	while(!feof(mapfile)) {
		for(i = 0; 4 > i; i++){
			fscanf(mapfile, "%f ", &map[k*4 + i]);
			map[k*4 + i]=map[k*4 + i]*scale;
		}
		k = k + 1;
		if(k >= MAX_MAPSIZE) break;
	}
	fclose(mapfile);
	// Turn geometric map into lines map
	getMapLines(map, k, 4, maplines);
	
	// Create big enough Vector for lines.
	cMap = newLineVector(k+100);
	cMap.NumOfLines = 0;
	for(i = 0; k > i; i++) {
		cMap.Lines[i].numOfPoints   = 0;
		cMap.Lines[i].p.numOfPoints = 0;
		cMap.Lines[i].p.a           = maplines[3*i];
		cMap.Lines[i].p.b		    = maplines[3*i+1];
		cMap.Lines[i].p.c			= maplines[3*i+2];
		cMap.Lines[i].x1            = map[4*i];
		cMap.Lines[i].y1            = map[4*i+1];
		cMap.Lines[i].x2            = map[4*i+2];
		cMap.Lines[i].y2            = map[4*i+3];
		cMap.NumOfLines++;
	}

	cMap.size = k + 100;

	return cMap;
}

/**
 * Inserts map points cMap.
 * 
 * @param cMap map where to add new lines.
 * @param map new lines, 4 points(x1, y1, x2, y2)
 * @param map_size, map array size is 4*map_size
 * @return 0 if all ok, else -1.
 */
int addLinesToMap(TLineVector *cMap, float *map, int map_size) {
	int i = 0;
	float maplines[3 * 150];
	
	//debug(DEBUGS,"MAP: cMap_size: %d map_size: %d NumOfLines: %d",
	//	cMap->size,map_size,cMap->NumOfLines);

	// Check parameters
	if(cMap == NULL || map == NULL || map_size > 150 || map_size <= 0) {
//		error(ERRS, "addLinesToMap: invalid parameter given.");
		return -1;
	}

	// Malloc enough space for new lines if needed
	if(map_size + cMap->NumOfLines > cMap->size) {
		cMap->Lines = (TLine*)realloc(cMap->Lines, (map_size + cMap->NumOfLines + 100)*sizeof(TLine));
		cMap->size = map_size + cMap->NumOfLines + 100;
	}

	// Turn geometric map into lines map
	getMapLines(map, map_size, 4, maplines);
	
	for(i = 0; map_size > i; i++) {
		cMap->Lines[cMap->NumOfLines].numOfPoints   = 0;
		cMap->Lines[cMap->NumOfLines].p.numOfPoints = 0;
		cMap->Lines[cMap->NumOfLines].p.a           = maplines[3*i];
		cMap->Lines[cMap->NumOfLines].p.b		   = maplines[3*i+1];
		cMap->Lines[cMap->NumOfLines].p.c		   = maplines[3*i+2];
		cMap->Lines[cMap->NumOfLines].x1            = map[4*i];
		cMap->Lines[cMap->NumOfLines].y1            = map[4*i+1];
		cMap->Lines[cMap->NumOfLines].x2            = map[4*i+2];
		cMap->Lines[cMap->NumOfLines].y2            = map[4*i+3];
		cMap->NumOfLines++;
	}

	return 0;
}

int saveMapDataToFile(TLineVector LV, const char *fileName,float scale){

#ifndef MAX_MAPSIZE
	#define MAX_MAPSIZE	4000
#endif

	//FILE *mapfile	  = fopen("mapAutLab.txt",    "r");	   // Map data as lines
	FILE *mapfile	  = fopen(fileName,    "w");	   // Map data as line
	
	int i=0;

	if(mapfile==NULL){
		return -1;
	}

	for(i=0;i<LV.NumOfLines;i++){
		fprintf(mapfile,"%.2f %.2f %.2f %.2f\n",LV.Lines[i].x1*scale,
												LV.Lines[i].y1*scale,
												LV.Lines[i].x2*scale,
												LV.Lines[i].y2*scale);
	}

	fflush(mapfile);
	fclose(mapfile);
	return 0;
}




/*****************************************************************
* getMapLines
* Converts geometric map, represented by end and start points of 
* line segment, to line parameters (a,b,c) ax+by+c=0
* 
********************************************************************/
void getMapLines(float *map, int M, int N, float *lines){
	int i,j;
	float x1,x2,y1,y2;
	
	j=0;
	for(i=0;i<M;i++){
		x1=map[j];
		y1=map[j+1];
		x2=map[j+2];
		y2=map[j+3];
		j=j+N;

		if(fabs(x2-x1)<0.001){
			lines[i*3]=1;
			lines[i*3+1]=0;
			lines[i*3+2]=-x1;
		}
		else{
			lines[i*3]=-(y2-y1)/(x2-x1);
			lines[i*3+1]=1;
			lines[i*3+2]=((y2-y1)/(x2-x1))*x1-y1;
		}
	}
}


/**
* int isCloseToEndPoints(TPose point, TLine Line)
*
* Checks if the TPose point (no heading used!) is close to the 
* line end points. Used for Local map determination.
* 
* Returns 1 if closer than LOCALMAPSIZE else 0
**/

int isCloseToEndPoints(TPose point, TLine Line){
	double mx1,mx2,my1,my2;
	double AC,AB;
	
	mx2=Line.x1; 
	my2=Line.y1; 
	mx1=Line.x2; 
	my1=Line.y2; 

	AC = (mx1-point.x)*(mx1-point.x) + (my1-point.y)*(my1-point.y);
	AB = (mx2-point.x)*(mx2-point.x) + (my2-point.y)*(my2-point.y);

	if (AC < LOCALMAPSIZE*LOCALMAPSIZE || AB<LOCALMAPSIZE*LOCALMAPSIZE)
		return 1;
	else return 0;
}

/**
* TLineVector getLocalMap(TPose pos,TLineVector map)
* 
* Finds local map around TPose pos from map
*
*/

TLineVector getLocalMap(TPose pos,TLineVector map){
	TLineVector lmap;
	int i;
	TPose point;
	int isIt;

	lmap = newLineVector(map.NumOfLines);
	if(lmap.size<=0){
	  fprintf(stderr,"getLocalMap: ERR: The map is not allocated properly\n");
	  return lmap;
	}
	for(i=0;i<map.NumOfLines;i++){
		
		if(pointDistanceFromLine(pos.x,pos.y,map.Lines[i])<LOCALMAPSIZE ){
			point = shortestPointFromLine(pos, map.Lines[i]);
			isIt = isInLine(point.x,point.y,map.Lines[i]);
			
			if(isIt==1){
				lmap.Lines[lmap.NumOfLines]=map.Lines[i];
				lmap.NumOfLines++;
			}
			else{
				if(isCloseToEndPoints(point,map.Lines[i])==1){
					lmap.Lines[lmap.NumOfLines]=map.Lines[i];
					lmap.NumOfLines++;
				}
			}//ELSE
		}//IF POINTDISTANCE
		
	}// FOR

	return lmap; 
}



TLineVector handleCadMap(TLineVector cadMap){
	TLineVector map;
	int i;
	int j;
	float lmin;
	float xmin;
	float xmax;
	//int shortestIndex;
	float l11,l12,l21,l22;
	TLine L = { 0, 0 };
	TLine L1 = {0, 0}, L2 = { 0, 0 };

	map = newLineVector(cadMap.NumOfLines);
	
	for(i=0;i<cadMap.NumOfLines;i++){
		
		if(lineLenght(cadMap.Lines[i])>0.2){
			map.Lines[map.NumOfLines] = cadMap.Lines[i];
			map.NumOfLines++;
		}
	}

	cadMap.NumOfLines=0;

	for(i=0;i<map.NumOfLines;i++){
		for(j=i+1;j<map.NumOfLines;j++){
			//if(angleDiff(map.Lines[i],map.Lines[j])<0.0175){
			
			if(map.Lines[i].x1!=0.0 && 
				(fabs( lineAngle(map.Lines[i])-lineAngle(map.Lines[j]) )<0.0175)){
				L1=map.Lines[i];
				L2=map.Lines[j];

				l11 = (L1.x1-L2.x1)*(L1.x1-L2.x1) + (L1.y1-L2.y1)*(L1.y1-L2.y1);
				l12 = (L1.x1-L2.x2)*(L1.x1-L2.x2) + (L1.y1-L2.y2)*(L1.y1-L2.y2);
				l21 = (L1.x2-L2.x1)*(L1.x2-L2.x1) + (L1.y2-L2.y1)*(L1.y2-L2.y1);
				l22 = (L1.x2-L2.x2)*(L1.x2-L2.x2) + (L1.y2-L2.y2)*(L1.x2-L2.x2);
				
				xmin = ___min( ___min(L1.x1,L1.x2),___min(L2.x1,L2.x2));
				xmax = ___max( ___max(L1.x1,L1.x2),___max(L2.x1,L2.x2));
				
				lmin = ___min(___min(l11,l12),___min(l21,22));

				if(L1.x1==xmin){
					L.x1=L1.x1;
					L.y1=L1.y1;
				}
				else if(L1.x2 == xmin){
					L.x1=L1.x2;
					L.y1=L1.y2;
				}
				else if(L2.x1==xmin){
					L.x1=L2.x1;
					L.y1=L2.y1;
				}
				else if(L2.x2==xmin){
					L.x1=L2.x2;
					L.y1=L2.y2;
				}
				
				if(L1.x1==xmax){
					L.x2=L1.x1;
					L.y2=L1.y1;
				}
				else if(L1.x2==xmax){
					L.x2=L1.x2;
					L.y2=L1.y2;
				}
				else if (L2.x1==xmax){
					L.x2=L2.x1;
					L.y2=L2.y1;
				}
				else if(L2.x2==xmax){
					L.x2=L2.x2;
					L.y2=L2.y2;
				}



				
				if(lmin<0.1){
					cadMap.Lines[cadMap.NumOfLines]=L;
					cadMap.NumOfLines++;
					map.Lines[j].x1=0;
					map.Lines[j].y1=0;
					map.Lines[j].x2=0;
					map.Lines[j].y2=0;
					map.Lines[j].numOfPoints=-1;
					j=map.NumOfLines;

				}
			}
			else if(j== map.NumOfLines-1 && map.Lines[i].x1!=0.0){
				cadMap.Lines[cadMap.NumOfLines] = map.Lines[i];
				cadMap.NumOfLines++;
			}
		}
	}
	
	map.NumOfLines=0;
	for(i=0;i<cadMap.NumOfLines;i++){
		
		if(lineLenght(cadMap.Lines[i])>0.3){
			map.Lines[map.NumOfLines] = cadMap.Lines[i];
			map.NumOfLines++;
		}
	}


	return map;
}

/****
* Checks if two lines have same a,b,c parameters 
* TODO: Scaling?!! 
**/
int isParametersExcactlySame(TLine l1,TLine l2){
	int is=0;
	if(l1.p.a==l2.p.a && l1.p.b==l2.p.b && l1.p.c==l2.p.c){
		//debug(DEBUGS,"YES THEY ARE\n");
		is=1;
	}

	return is;
}



TLineVector joinMapLines(TLineVector map){
	TLineVector jMap;
	int i,j;
	TLine joined;
	int ind1,ind2;
	int isJoined;
	
	jMap = newLineVector(map.NumOfLines);
	
	for(i=0;i<map.NumOfLines;i++){
		isJoined=0;
		for(j=i+1;j<map.NumOfLines;j++){
			if(isJoined==0){
				
				if(isParametersExcactlySame(map.Lines[i],map.Lines[j])){
					
					if(shortestDistBetweenLineEndPoints(map.Lines[i],map.Lines[j])<0.3f*0.3f){
						joined=mergeTwoLines(map.Lines[i],map.Lines[j],ind1,ind2);
						addLine2Vector(jMap,joined,jMap.NumOfLines);
						isJoined++;
					}
				}
			}

		}//FOR j
		
		if(isJoined==0){
			addLine2Vector(jMap,map.Lines[i],jMap.NumOfLines);
		}
	}// FOR i


	return jMap;
}
#ifdef MAIN
// Handles the cadmap

int main(){
	TLineVector origMap;
	TLineVector jMap;

	origMap = readMapDataFromFile("2kerros.txt",1.0f/1000.0f);
	jMap=joinMapLines(origMap);
	debug(DEBUGS,"N_ORIG:%d N_JOIN:%d\n",origMap.NumOfLines,jMap.NumOfLines);

freeLineVector(jMap);
freeLineVector(origMap);
return 0;
}
#endif
